#ifndef __HARDWAREINTERFACE_H
#define __HARDWAREINTERFACE_H

#include <future>
#include "rclcpp/rclcpp.hpp"
#include <math.h>
#include <geometry_msgs/msg/twist.hpp>
#include <dynamic_reconfigure/server.h>
#include <driver/paramsConfig.h>
#include <motor/motor.h>
#include <encoder/encoder.h>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <diagnostic_msgs/msg/diagnostic_status.hpp>
// #include <agv_msgs/msg/agv_event.hpp>
#include <agv_msgs/msg/driver_state.hpp>
#include <agv_msgs/msg/drivers_info.hpp>
#include <resource_manager/joint_handle.h>
#include <agv_srvs/srv/motor_enable.hpp>
#include <common/event_publisher.h>

#define ROS_NODEID (1)

#define EVENT_PARAM_ID_INIT_ERROR  				101
#define EVENT_PARAM_ID_RECOVER_ERROR  			102

namespace ros_canopen_driver {


/// \brief Hardware interface for a robot
class HardwareInterface{
public:

	/**
	 * \brief Constructor
	 * \param nh - Node handle for topics.
	 */
	HardwareInterface(rclcpp::Node& nh);

    /// \brief Destructor
	~HardwareInterface();

	void init();

	void read();

	void write();

	/// \brief Do servo Init.
	bool servoInit();
	bool servoInitSingle(std::string joint_name);

	/// \brief Do servo Recover.
	bool servoRecover();
	bool servoRecoverSingle(std::string joint_name);

	/// \brief Do servo Shutdown.
	bool servoShutdown();

	/// \brief Do servo Halt.
	bool servoHalt();
	bool servoHaltSingle(std::string joint_name);

	void quit();

	bool hasError();

	rclcpp::Time getTime() const { return rclcpp::Time::now(); }
  	rclcpp::Duration getPeriod() const { return rclcpp::Duration(0.01); }

  	resource_manager::JointHandleCollector* getJointHandleCollector() { return &jhc_;}
	common::EventPublisher  			ep_;


  	void maunVelCallback(const geometry_msgs::msg::Twist &command);
	bool motor_enable_callback(agv_srvs::srv::MotorEnable::Request &req, agv_srvs::srv::MotorEnable::Response &resp);
	void publish_diagnostics(unsigned char level, std::string hardware_id, std::string message);
	bool callNmtService(uint32_t value);
	double getJointTimestamp(std::string joint_name);
	double getJointOldestTimestamp();


private:
	ros::Publisher diagnostics_pub_;
	diagnostic_msgs::msg::DiagnosticArray diagnostics_;

	ros::Publisher event_pub_;
	ros::Publisher driversInfo_pub_;
    agv_msgs::msg::DriverState driverState_;
    agv_msgs::msg::DriversInfo driversInfo_;
	std::vector<agv_msgs::msg::DriverState> driverStatus;
	rclcpp::Timer event_timer_;

	ros::ServiceServer srv_motor_enable_;
    ros::ServiceClient  set_entry_cln_;
protected:

	// Startup and shutdown of the internal node inside a roscpp program
	rclcpp::Node nh_;

	resource_manager::JointHandleCollector jhc_;
	
	bool velocity_interface_running_;
	bool position_interface_running_;
	// Shared memory
	std::vector<std::string> joint_names_;
	std::vector<double> joint_position_;
	std::vector<double> joint_velocity_;
	std::vector<double> joint_effort_;
	std::vector<double> joint_timestamp_;
	std::vector<double> joint_position_command_;
	std::vector<double> joint_velocity_command_;
	std::vector<double> joint_effort_command_;
    std::size_t num_joints_;

	typedef dynamic_reconfigure::Server<driver::paramsConfig> ReconfigureServer;
	std::shared_ptr<ReconfigureServer> dyn_reconf_server_;
	boost::recursive_mutex dyn_reconf_server_mutex_;
   
	std::vector<Motor*> 				servo_;
	std::vector<encoder::Encoder*> 		encoder_;

	// int seq_;

};
// class

}// namespace

#endif
